//
// Created by Raymoond Tang on 19/4/13.
// Copyright (c) 2013 NUS. All rights reserved.
//
// To change the template use AppCode | Preferences | File Templates.
//



#ifndef __VelocityTuning_H_
#define __VelocityTuning_H_

#include <iostream>
#include "CollisionRange.h"
#include "PlayerObject.h"
#include <vector>
#include <list>
#include <math.h>
#include <boost/shared_ptr.hpp>
#include "GlobalStructure.h"

using namespace std;

class VelocityTuning {
private:

    double MaxVelocity;

    Robot2D velocity;

    pair<double,double> sol;

    Robot2D goal;

public:
    VelocityTuning():MaxVelocity(0){}

    VelocityTuning(double max_vel):MaxVelocity(max_vel){};

    ~VelocityTuning();
    
    double waitTime;

    void updateState(Robot2D robot_pos, Segment2D path, const std::vector<PlayerObject>& obstacles);
    
    void updateState(Robot2D robot_pos, std::vector<Position> path,std::vector<PlayerObject> obstacles);

//    player_pose2d getVelocity();
    Robot2D getVelocity(double time);

    Robot2D getVelocity(double time,const Robot2D &pos);
    
    Robot2D getVelocity(Segment2D path);
    
    pair<double,double> getSol();

    void joinRange(list<pair <double, double> > &v_set, pair<double, double> new_range);

    pair<double,double> locateSolRange(list<pair <double,double> > &v_set,double timeNeeded);

};

static double DotProduct(Robot2D start, Robot2D end, Robot2D curr) {
    Robot2D a = end - start;
    Robot2D b = curr - end;

    return a.x*b.x + a.y * b.y;
}

static bool ExceededTarget(Robot2D start, Robot2D end, Robot2D curr) {
    bool exceeded =  DotProduct(start, end, curr) >= 0.0;
    cout << start.toString() << endl;
    cout << end.toString() <<endl;
    cout << curr.toString() <<endl;
    cout<< exceeded << endl;
    return exceeded;
}


#endif //__VelocityTuning_H_
